1. Field of the Invention
The present invention relates generally to robotic control systems and more particularly to hybrid position and force control systems of the kind that are used for controlling robotic manipulators.
2. The Prior Art
Manipulators having end effectors such as robotic "hands" find many uses. For example, such a manipulator might install an electronic component having many leads on a printed circuit board with each lead in the correct hole for soldering. Another might perform a machining operation such as cutting or grinding a workpiece. Manipulators are particularly well suited for performing manual operations in hostile environments such as outer space or a radioactive area of a nuclear reactor where humans cannot readily go. Frequently the operations performed by a manipulator involve interaction with the environment under circumstances requiring great precision both in positioning the end effector and in the amount of force it exerts. Accordingly there has been a need for an efficient and effective means to control the operation of these manipulators.
FIG. 1 provides an example of a typical manipulator generally 100. An end effector such as a robotic hand 101 is rotatingly coupled to a link 103 by a joint 105. The link 103 is pivotingly coupled by a joint 107 to another link 109, and the link 109 in turn is pivotingly coupled by a joint 111 to another link 113. The link 113 is coupled through a joint 115 to an upper vertical link 117. The link 117 is rotatingly and telescopingly coupled by a joint 119 to a lower vertical link 121 which is supported by a base 123. The manipulator is controlled by a controller 125 which typically includes a communication terminal 127 and calculating and memory circuits (not shown).
As shown in cutaway view, motive means such as an electric motor 129 rotates the upper vertical link 117 with respect to the lower vertical link 121 as indicated by an arrow 131. Similarly, other motive means (not shown) such as motors or pneumatic actuators move the various other links in varying ways according to the physical structure of the joints. A sensor such as an optical encoder 133 provides feedback respecting the angular orientation of the link 117 with respect to the link 121. Other sensors (not shown) provide feedback respecting the positions of the various other joints with respect to their associated links or other parts. A sensor such as a strain gage 135 provides feedback respecting the torque being exerted by the upper link 117. Other sensors (not shown) provide similar feedback respecting the forces and torques being exerted by other parts of the manipulator.
The position and orientation of the hand 101 are typically described relative to some convenient coordinate system of interest. For example, for the manipulator 100 it might be convenient to use a fixed Cartesian coordinate system having an origin 137 at a desired location, an x-axis 139 and a y-axis 141 which define a horizontal plane parallel to the base 123, and a vertical z-axis 143 parallel to the vertical link 117. In other applications it might be preferable to use another coordinate system such as spherical or cylindrical or a system in which the origin is not fixed.
The required motion of the end effector of a robotic manipulator is often very complex, and various systems have been proposed for accurately and automatically controlling this motion. One such system, known as a "hybrid" control system, provides unified control of both the displacement of an end effector and the forces exerted thereby (for convenience, the term "displacement" will be used herein to refer to the position and orientation of an end effector).
In general, a hybrid control system controls either displacement or force in any one direction of motion. The main concept is to control force in directions in which it is difficult to control displacement, for example because the geometry is not well defined or is unknown or because a certain contact force is required. Hybrid control systems have functioned satisfactorily in specialized applications, but in general such systems have not provided stable control. This lack of guaranteed stability has limited the applicability and acceptability of hybrid control systems.
FIG. 2 provides a functional block diagram of a prior art hybrid control system as proposed by Craig, J. J., and M. H. Raibert in "A Systematic Method of Hybrid Position/Force Control of a Manipulator", Computer Software and Applications Conference, IEEE Computer Society, November 1979, Chicago, Ill., pages 446-451. The purpose of this control system is to control both the displacement of, and the force applied by, an end effector of a manipulator 11. It will be understood that the manipulator 11 may represent any of a number of different kinds of manipulators, of which the manipulator 100 shown in FIG. 1 is merely exemplary. Similarly, the manipulator 11 may have any of various kinds of end effectors. The nearly symmetrical upper and lower halves of FIG. 2 depict displacement and force control portions, respectively, of the hybrid control system.
In the upper half of FIG. 2, a summing node 13 receives a desired displacement vector x.sub.d that represents a desired displacement of the end effector of the manipulator 11 and an actual displacement vector x.sub.a that represents the actual displacement thereof. As indicated above, each of these displacement vectors represents both position and orientation of the end effector and therefore typically is a six-element vector. In some applications less than six elements may be required; for example, in the case of an end effector that moves entirely in a plane, a three-element vector giving x and y coordinates and a single orientation value would be sufficient.
In some situations it may be desirable to enhance system performance by further describing the motion of the end effector, for example by means of one or more other vectors such as velocity and acceleration in addition to the displacement vector.
In any given situation, particular values of the elements of the desired displacement vector x.sub.d are provided by a user or may be calculated by a computer or the like. The values of the elements of the actual displacement vector x.sub.a are derived from signals provided by sensors such as those discussed with reference to the manipulator 100 that detect the actual positions and orientations of the various joints of the manipulator or of the end effector itself.
The summing node 13 algebraically combines the actual and desired displacement vectors x.sub.a and x.sub.d and produces an end effector displacement error vector x.sub.e indicative of any difference; that is, EQU x.sub.e =x.sub.d -x.sub.a.
Control signals are derived from x.sub.e and are used to move the end effector toward its desired position and orientation, as will be explained in more detail shortly.
Similarly, in the lower half of FIG. 2 a summing node 15 receives a desired force vector f.sub.d that represents a desired force to be exerted by the end effector of the manipulator 11 and an actual force vector f.sub.a that represents the actual force exerted by the end effector. These force vectors actually represent both force and moment information and therefore, like the displacement vectors, they typically have six elements.
Particular values of the elements of the vectors f.sub.d and f.sub.a are determined in a similar manner to that which is used to determine values of the elements of the vectors x.sub.d and x.sub.a, respectively.
The summing node 15 algebraically combines the actual and desired force vectors f.sub.a and f.sub.d and produces an end effector force error vector f.sub.e indicative of any difference, in a manner analogous to the determination of the end effector displacement error vector x.sub.e as previously described. Control signals are derived from f.sub.e and are used to cause the end effector to exert the desired force.
The method by which the end effector displacement and force error vectors x.sub.e and f.sub.e are used to generate the control signals will now be described in more detail.
It will be necessary to select those terms of x.sub.e that correspond to axes along which the displacement of, and not the force exerted by, the end effector is to be controlled. As indicated by a function block 17, this selection is accomplished by means of a diagonal selection matrix (designated S).
Each diagonal element of the selection matrix S has a value of either one or zero. A value of one indicates an axis along which displacement is to be controlled, whereas a value of zero indicates an axis along which displacement is not to be controlled. The matrix S is typically a 6.times.6 diagonal matrix.
The vector x.sub.e is multiplied by the matrix S to obtain a selected end effector displacement error vector x.sub.e.sbsb.s as follows: EQU x.sub.e.sbsb.s =Sx.sub.e ( 1)
The vector x.sub.e.sbsb.s must be converted into values representative of displacements of the various joints of the manipulator. As indicated by a function block 19, this conversion is accomplished by means of an inverse of a certain matrix known as the Jacobian matrix.
The Jacobian matrix (designated J) is a first order approximation of a function that maps differential motions of the joints of a manipulator, represented by a joint displacement error vector .theta..sub.e, into differential motions of the end effector in terms of the coordinate system of interest. The differential motions of the end effector, of course, are represented by the end effector displacement error vector x.sub.e. This mapping is expressed in the following linearized relationship: EQU x.sub.e =J.theta..sub.e ( 2)
The vector .theta..sub.e has one element associated with each joint; if the robot has n joints then the vector .theta..sub.e is an n-element vector. The derivation of the Jacobian matrix J is given, for example, in Paul, R. P., Robot Manipulators: Mathematics, Programming and Control, MIT Press, 1981.
If the vector .theta..sub.e is already known, then the vector x.sub.e may be calculated according to Eq. (2). But in the present situation the vector x.sub.e is known and it is desired to calculate the vector .theta..sub.e ; therefore, what is required is an inverse of the mapping provided by the matrix J. A unique inverse mapping exists if J is a square matrix of maximal rank, in which event a joint displacement error vector .theta..sub.e may be calculated from the end effector displacement error vector x.sub.e as follows: EQU .theta..sub.e =J.sup.-1 x.sub.e ( 3)
Eq. (3) has been used in an attempt to compute a desired selected joint displacement error vector .theta..sub.e.sbsb.s from the selected end effector displacement error vector x.sub.e.sbsb.s as determined in Eq. (1) as follows: EQU .theta..sub.e.sbsb.s =J.sup.-1 x.sub.e.sbsb.s ( 4)
However, calculating the selected joint displacement error vector .theta..sub.e.sbsb.s in this manner can result in instability and therefore limits the usefulness of the hybrid control system.
It will be apparent to those skilled in the art that if J is not square, the pseudo-inverse J.sup.+ would be used in Eqs. (3) and (4) in place of the inverse J.sup.-1. For simplicity, the term J.sup.-1 will be used herein to denote the inverse of J if J is square and the pseudo-inverse of J if J is not square.
When the selected joint displacement error vector .theta..sub.e.sbsb.s has been computed, it is used to calculate elements of a position-correcting torque vector .tau..sub.p as indicated by a "position control law" function block 21. The vector .tau..sub.p represents control signals to be applied to the motive means of the joints to cause the end effector to move to the desired position and orientation.
The position control law typically is a linear differential equation, but it could be any function that operates upon an input vector to produce a unique output. In practice, the control law is often a P-I-D (proportional, integral, differential) control law, although an adaptive or other type of control law could be used instead.
Analogously to selecting terms of x.sub.e corresponding to axes along which the position of the end effector is to be controlled, terms of the end effector force error vector f.sub.e corresponding to axes along which the force exerted by the end effector is to be controlled must also be selected. This selection is accomplished by means of the matrix I-S (where I is the identity matrix) as indicated by a function block 23. The matrix I-S is the orthogonal complement of the selection matrix S.
Each diagonal element of the matrix I-S has a value of either one or zero. A value of one indicates an axis along which force is to be controlled, whereas a value of zero indicates an axis along which force is not to be controlled. In the embodiment being described, this matrix typically is a 6.times.6 diagonal matrix. Force control is orthogonal to position control and therefore the mathematical definition is EQU S.sup..perp. =I-S.
It should be understood that the zeros in the diagonal of S correspond with the ones in the diagonal of S.sup..perp. and vice versa.
The vector f.sub.e is multiplied by the matrix S.sup..perp. to obtain a selected end effector force error vector f.sub.e.sbsb.s as follows: EQU f.sub.e.sbsb.s =s.sup..perp. f.sub.e
The values of the end effector selected force error vector f.sub.e.sbsb.s must be converted into torques to be exerted by the various joints. This is done by means of J.sup.T, the transpose of the Jacobian matrix J, according to the following relation: EQU .tau..sub.e =J.sup.T f.sub.e.
The preceding equation, unlike Eq. (3), is not an approximation and therefore is correct for any J and f. It follows that a desired joint force error vector .tau..sub.e.sbsb.s may be calculated according to the following relation: EQU .tau..sub.e.sbsb.s =J.sup.T f.sub.e.sbsb.s
as indicated by a function block 25, without introducing any instability into the system.
After the joint force error vector .tau..sub.e.sbsb.s has been computed, it is used to calculate elements of a force-correcting torque vector .tau..sub.f as indicated by a "force control law" function block 27. The vector .tau..sub.f represents control signals to be applied to the motive means of the joints to cause the end effector to exert the desired force.
Finally, the position-correcting torque vector .tau..sub.p and the force-correcting torque vector .tau..sub.f are algebraically combined, as indicated by a summing node 29, to produce a final set of control signals that are applied to the motive means associated with the various joints of the manipulator so as to cause the end effector to move to the desired position and orientation and to exert the desired force and moment.
FIG. 2 shows a typical means to calculate the actual displacement and force vectors x.sub.a and f.sub.a. Displacements of the various joints as measured by sensors are represented by a joint vector .theta..sub.a. The actual displacement vector x.sub.a is computed from the joint vector .theta..sub.a according to a forward kinematics equation as indicated by a function block 31. Similarly, as indicated by a function block 33, the actual force vector f.sub.a is computed from the measured forces f.sub.h such as those measured by the sensors at the various joints by means of a force transform equation.
The hybrid control system as described above is typical of various kinds of hybrid control systems that have been proposed. Although these systems have functioned satisfactorily in some special applications, in general they can become unstable for certain combinations of S and J.sup.-1. That is, an infinite (or near-infinite) or unbounded oscillatory position-correcting torque vector .tau..sub.p may be produced in response to finite values of x.sub.e.sbsb.s. This potential for unstable operation has limited the applicability and acceptability of hybrid position and force control systems. Accordingly there is a need for a way to realize a stable hybrid control system for a robotic manipulator.